package org.noote.libs.robot.stepper;

import org.noote.libs.robot.Robot_Animation;
import org.noote.libs.robot.Robot_AnimationContext;
import org.noote.libs.robot.Robot_ServoMotor;
import org.noote.libs.robot.Robot_Skeleton;
import org.noote.libs.robot.stepper.StepperRobot_AnimationStep.Unit;

public class StepperRobot_AnimationContext extends Robot_AnimationContext {

	protected int _iStep=0;
	protected float _fStepTime = 0;
	
	StepperRobot_Animation _anim;
	StepperRobot_AnimationStep _step0, _step1;
	
	public StepperRobot_AnimationContext(Robot_Skeleton robot) {
		super(robot);
	}
	
	@Override
	public boolean start(Robot_Animation anim)
	{
		return false;
	}
	
	public boolean start(StepperRobot_Animation anim)
	{
		if(!super.start(anim)) return false;
		
		_iStep=0;
		_fStepTime=0;
		_anim=anim;
		if(_anim._steps.size()==0)
			_bFinished=true;
		else if(_anim._steps.size()>1)
			setStep(_anim._steps.get(_iStep), _anim._steps.get(_iStep+1));
		else
			setStep(_anim._steps.get(_iStep), _anim._steps.get(_iStep));
		
		return doStep();
	}
	
	@Override
	public void addTime(float fSlice) {
		super.addTime(fSlice);
		_fStepTime=fSlice;
		
		// step is over ?
		if(doStep())
			nextFrame();
	}
	
	protected void setStep(StepperRobot_AnimationStep step0, StepperRobot_AnimationStep step1)
	{
		_step0 = step0;
		_step1 = step1;
	}

	protected boolean nextFrame() {
		_fStepTime = 0;
		if(_iStep+1>=(_anim._steps.size()-1))
		{
			// animation is finish ?
			_bFinished = true;
			return false;
		}
		_iStep++;
		setStep(_anim._steps.get(_iStep), _anim._steps.get(_iStep+1));
		return true;
	}
	
	protected boolean doStep()
	{
		if(_fStepTime == 0)
		{
			for(Robot_ServoMotor servo : _robot.getServoList())
			{
				Unit u = _step0.find(servo.getID());
				if(u != null)
				{
					servo.setPosition(u._fPosition);
				}
			}
		}
		else if(_fStepTime>=_step0._fDuration)
		{
			for(Robot_ServoMotor servo : _robot.getServoList())
			{
				Unit u = _step1.find(servo.getID());
				if(u != null)
				{
					servo.setPosition(u._fPosition);
				}
			}
			
			// end step !
			return true;
		}
		else
		{
			float sLerp = (_step0._fDuration-_fStepTime)/_step0._fDuration;
			for(Robot_ServoMotor servo : _robot.getServoList())
			{
				Unit u0 = _step0.find(servo.getID());
				Unit u1 = _step1.find(servo.getID());
				if(u0 != null && u1 != null && u0._fPosition!=u1._fPosition)
				{
					float r = (u0._fPosition*sLerp) + (u1._fPosition*(1.0f-sLerp));
					
					servo.setPosition(r);
				}
				else if(u0 != null)
				{
					servo.setPosition(u0._fPosition);
				}
				else if(u1 != null)
				{
					servo.setPosition(u1._fPosition);
				}
			}
		}

		return false;
	}
}
